#define MCAP_IMPLEMENTATION // Define this in exactly one .cpp file
#include "log_mcap.h"

#include <chrono>
#include <mcap/writer.hpp>
#include <thread>

#include <chrono>
#include <iostream>
#include <vector>

#include <google/protobuf/descriptor.h>
#include <google/protobuf/descriptor.pb.h>

double sequence = 0;

LogMcap::LogMcap() {
  // Constructor implementation
}

LogMcap::~LogMcap() {
  // Destructor implementation
}

void wait_seconds(int seconds) {
  std::this_thread::sleep_for(std::chrono::seconds(seconds));
}

// Returns the system time in nanoseconds.
mcap::Timestamp now() {
  return mcap::Timestamp(
      std::chrono::duration_cast<std::chrono::nanoseconds>(
          std::chrono::system_clock::now().time_since_epoch())
          .count());
}

bool LogMcap::write_mcap_head() {
  // mcap::McapWriter writer;
  // 尝试打开一个名为"output.mcap"的文件进行写入，使用ROS1兼容的选项
  auto status = writer.open("output.mcap", mcap::McapWriterOptions("ros1"));
  // 检查文件打开是否成功
  if (!status.ok()) {
    // 如果打开失败，输出错误信息到标准错误流
    std::cerr << "Failed to open MCAP file for writing: " << status.message
              << "\n";
    // 返回，终止程序执行
    return false;
  }
  const google::protobuf::FileDescriptor *file_desc =
      plannintg_mcap_debug::plannintg_mcap_debug::descriptor()->file();

  // 创建文件描述符集合
  google::protobuf::FileDescriptorSet fd_set;
  file_desc->CopyTo(fd_set.add_file());

  // 序列化为字符串
  std::string schema_data;
  fd_set.SerializeToString(&schema_data);

  // 使用正确的schema数据
  mcap::Schema plannintg_mcap_debug_schema(
      "plannintg_mcap_debug.plannintg_mcap_debug", "protobuf", schema_data);

  writer.addSchema(plannintg_mcap_debug_schema);

  mcap::Channel plannintg_mcap_debug_channel("/PlannerMcapLog", "protobuf",
                                             plannintg_mcap_debug_schema.id);

  writer.addChannel(plannintg_mcap_debug_channel);
  plannintg_mcap_debug_channel_ = plannintg_mcap_debug_channel;

  return true;
}
bool LogMcap::write_mcap_tail() {
#if !defined(MCAP_ENV_OK) || !defined(__x86_64__)
  return false;
#endif

  std::string payload;
  // 检查序列化是否成功
  bool serialized = plannintg_mcap_debug.SerializeToString(&payload);
  if (!serialized) {
    std::cerr << "Failed to serialize plannintg_mcap_debug message\n";
    return false;
  }

  mcap::Message msg;
  msg.channelId = plannintg_mcap_debug_channel_.id;
  msg.sequence = 1;
  msg.logTime = now();
  msg.publishTime = msg.logTime;
  msg.data = reinterpret_cast<const std::byte *>(payload.data());
  msg.dataSize = payload.size();
  auto status = writer.write(msg);
  if (!status.ok()) {
    std::cerr << "Failed to write message: " << status.message << "\n";
  }

  writer.close();

  return true;
}

int main() {
  LogMcap log_mcap;
  log_mcap.write_mcap_head();
  double sequence = 2;
  for (int i = 0; i < 10; i++) {
    log_mcap.plannintg_mcap_debug.set_x(1.0 + sequence);
    log_mcap.plannintg_mcap_debug.set_y(2.0);
    log_mcap.plannintg_mcap_debug.set_v(3.0);
  }
  log_mcap.write_mcap_tail();
  return 0;
}